#!/usr/bin/python
import rospy
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import TransformStamped
import tf_conversions
import tf2_ros

global pub

def callback(data):
    br = tf2_ros.TransformBroadcaster()
    t = TransformStamped()

    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "map"
    t.child_frame_id = "com"
    t.transform.translation.x = data.pose[2].position.x
    t.transform.translation.y = data.pose[2].position.y
    t.transform.translation.z = data.pose[2].position.z
    t.transform.rotation.x = data.pose[2].orientation.x
    t.transform.rotation.y = data.pose[2].orientation.y
    t.transform.rotation.z = data.pose[2].orientation.z
    t.transform.rotation.w = data.pose[2].orientation.w

    br.sendTransform(t)

if __name__ == "__main__":
    rospy.init_node("model_state_to_tf")

    from_topic = rospy.get_param("from_topic", "/gazebo/model_states")

    rospy.Subscriber(from_topic, ModelStates, callback, queue_size=10)

    rospy.spin()